#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <assert.h>

#include "ControlledVertibrateObject.h"
#include "Sprite.h"
#include "GL/glut.h"
#include "include.h"

#define abs(num) num<0?num*-1.0:num

ControlledVertibrateObject::~ControlledVertibrateObject()
{

}
ControlledVertibrateObject::ControlledVertibrateObject( TextureManager * textureManager,
					 			Vector2D position,
					  			Vector2D * vertices,
					  			int numVertices,
					  			float mass,
				      			bool isGrabbable,
				      			char * skeletonFile)
:VertibrateObject(textureManager, position, vertices, numVertices, mass, isGrabbable, skeletonFile)
{
	this->textureManager = textureManager;
	forceDamageDenominator = 1000000.0;
	health = 100;
	maxHealth = 100;
	lastRotation = 0.0;
	isNotOnGroundCount = 0;
	movingTimeModifier = 50.0;
	isOnGroundCount = 0;
	isOnLeftWallCount = 0;
	isOnRightWallCount = 0;
	facingRight = true;
	movingLeft = false;
	movingRight = false;
	objectId = CONTROLLEDOBJECT_CLASS;
	spriteList = new SpriteList();
	rotateCount = 0;
	isOnGround = false;
	hitGround = false;
	onGroundFilter = 0;
	isOnRightWall = false;
	isOnLeftWall = false;
	translationalForce = Vector2D(0,0);
	torque = 0;
	collisionValid = false;
	numCollisions = 0;
}
void ControlledVertibrateObject::SetHealth(float health)
{
	this->health = health;
}
float ControlledVertibrateObject::GetHealth()
{
	return health;
}
void ControlledVertibrateObject::AddForceObjectSpace(Vector2D ray, Vector2D origin, bool groundForce)
{
	if(ray.Magnitude() == 0.0)
		return;
	
	float forceStrength = ray.Magnitude()/forceDamageDenominator;
	
	
	if(forceStrength > 1 && (origin.dist(vertices[2]) > 0.1 && origin.dist(vertices[3]) > 0.1))
	{
		health -= forceStrength;
	}
	Vector2D towardCenter = -1.0 * origin;
	Vector2D unitTowardCenter = !towardCenter; 
	Vector2D unitRay = !ray;

	float cosTheta = ray.dot(unitTowardCenter);//unitTowardCenter.dot(ray);
	
	Vector2D rayTowardCenter = (cosTheta) * unitTowardCenter;
	//rotation = 0.0;

	if(GetFreeRotate())
	{
		AddForce(rayTowardCenter);
		Vector2D rayRotational = (ray - rayTowardCenter);
		torque += rayRotational.x * towardCenter.y - rayRotational.y * towardCenter.x;
	}
	else
	{
		AddForce(ray);
	}
	
	//If ray is "mostly up" we should add friction and move the bottom vertices so
	//we're touching the ground.
	if(groundForce)
	{
		float angle = atan2(ray.y, ray.x);

			//the incline angle he is capable of running up should be dependent on his
		//velocity in his forward direction.
		Vector2D horizontal = !(vertices[1] - vertices[0]);
		float horizontalVelocity = horizontal.dot(velocity);
		if(horizontalVelocity < 0)
			horizontalVelocity *= -1;
		//30 degrees = 0.523598333 radians
		float smallAngle = 45 - horizontalVelocity/600.0 * 135;
		//if(smallAngle < 20)
		//	smallAngle = 20;
		float largeAngle = 180 - smallAngle;
		smallAngle *= PI/180;
		largeAngle *= PI/180;
		
		if(smallAngle > largeAngle)
		{
			float temp = smallAngle;
			smallAngle = largeAngle;
			largeAngle = temp;
		}
	
		if(angle >= smallAngle && angle <= largeAngle)
		{
			hitGround = true;
			isOnGround = true;
			isOnRightWall = false;
			isOnLeftWall = false;
			if(!GetFreeRotate())
			{
				Vector2D objectUp = vertices[1] - vertices[2];
				float bottomAngle = atan2(objectUp.y, objectUp.x);//acos(orthogonal.dot(bottom));
				bottomAngle = angle - bottomAngle;
				
				rotation += bottomAngle;
				rotateCount++;
				if(true)//horizontalVelocity > 100)
				{
					Vector2D down = !(vertices[1] - vertices[2]);
					down = down * (horizontalVelocity/600) * 2 *GRAVITATIONAL_FORCE * mass;
					//AddForce(down);
					
				}
			}
		}
		else if(!movingRight && !isOnGround && (angle >= -smallAngle && angle < smallAngle))// && !isOnRightWall && !isOnGround)
		{
			isOnLeftWall = true;
			Vector2D objectSide = vertices[1] - vertices[0];
			float sideAngle = atan2(objectSide.y, objectSide.x);
			sideAngle = angle - sideAngle;
			
			//rotation += sideAngle;
			//rotateCount++;
		}
		else if(!movingLeft && !isOnGround && (angle <= -largeAngle || angle > largeAngle))// && !isOnGround && !isOnLeftWall)
		{
			isOnRightWall = true;
			if(angle < 0)
				angle += 2 * 3.14159;
			Vector2D objectSide = vertices[0] - vertices[1];
			float sideAngle = atan2(objectSide.y, objectSide.x);
			sideAngle = angle - sideAngle;
			
			//rotation += sideAngle;
			//rotateCount++;
		}
		else
		{
			isOnHead = true;
		}
		
		if(isOnRightWall || isOnLeftWall)
		{
			Vector2D wallFriction = !velocity;
			wallFriction = -WALL_FRICTION_FORCE * wallFriction * mass;
			Vector2D right = vertices[1] - vertices[0];
			right = !right * WALL_FRICTION_FORCE * mass;
			if(!isOnRightWall)
				right = right * -1;
			AddForce(right);
		}
	}
}
void ControlledVertibrateObject::AddForceObjectSpace(Vector2D ray, Vector2D origin)
{
	AddForceObjectSpace(ray, origin, true);
}

bool ControlledVertibrateObject::GetFreeRotate()
{
	return false;
}

void ControlledVertibrateObject::Rotate(float theta)
{
	if(PreRotate(theta))
	{
		return;
	}
	
	if(isPinned)
		return;
		
	if(1)//GetFreeRotate())
	{
		while(theta < 0)
			theta += 2.0 * 3.14159;
			
		if(abs(theta) < 0.000001)
			return;
		float sinTheta = sin(theta);
		float cosTheta = cos(theta);
		
		for(int i = 0; i < numVertices; i++)
		{
			float newX = vertices[i].x * cosTheta - vertices[i].y * sinTheta;
			float newY = vertices[i].y * cosTheta + vertices[i].x * sinTheta;
			vertices[i].x = newX;
			vertices[i].y = newY;
		}	
		
		float x = upVector.x * cosTheta - upVector.y * sinTheta;
		float y = upVector.y * cosTheta + upVector.x * sinTheta;
		upVector.x = x;
		upVector.y = y;
		
		if(keyPointList)
		{
			keyPointList->Rotate(sinTheta, cosTheta);
		}
		if(collisionValid)
		{
			float x = collisionPoint.x * cosTheta - collisionPoint.y * sinTheta;
			float y = collisionPoint.y * cosTheta + collisionPoint.x * sinTheta;
			collisionPoint.x = x;
			collisionPoint.y = y;
		}
		
		ControlledObjectRotateEntryPoint(cosTheta, sinTheta);
	}
}
float ControlledVertibrateObject::GetAnimationSpeed()
{
	Vector2D horizontal = !(vertices[1] - vertices[0]);
	float horizontalVelocity = horizontal.dot(velocity);
	if(horizontalVelocity < 0)
		horizontalVelocity *= -1;
		
	if(isOnGround)
		return horizontalVelocity/movingTimeModifier;
	return 1.0;
}

bool ControlledVertibrateObject::VertibrateObjectPreDrawEntryPoint()
{	
	ControlledObjectDrawEntryPoint();
	
	if(!isOnGround && isOnRightWall)
	{
		//on the right wall
		skeleton->SetDirection(false);
		skeleton->SetAnimation("wallJump",0,0,1);
	}
	else if(!isOnGround && isOnLeftWall)
	{
		//on the left wall
		skeleton->SetDirection(true);
		skeleton->SetAnimation("wallJump",0,0,1);
	}
	else
	{
		if(!isOnGround)
		{
			skeleton->SetAnimation("flail",0,0,1);	
		}
		else
		{
			Vector2D horizontal = !(vertices[1] - vertices[0]);
			float horizontalVelocity = horizontal.dot(velocity);
			
			if(horizontalVelocity > 10 || horizontalVelocity < -10)
			{
				if(horizontalVelocity > 200.0 || horizontalVelocity < -200.0)
				{
					skeleton->SetAnimation("run",0,0,1);
				}
				else
					skeleton->SetAnimation("walk",0,0,1);
				skeleton->SetTimeModifier(GetAnimationSpeed());
			}
			else
			{
				skeleton->SetAnimation("stand",0,0,1);
				skeleton->SetTimeModifier(1.0);
			}
		}
		if((GetFreeRotate()||(facingRight && velocity.x < -10) || (!facingRight && velocity.x < 10)))
		{
			facingRight = false;
			
		}
		else
			facingRight = true;
		skeleton->SetDirection(facingRight);
	}
	
	spriteList->Draw();
	return false;
}

void ControlledVertibrateObject::MoveRight()
{
	Vector2D right = vertices[1] - vertices[0];
	right = !right;
	float rightSpeed = right.dot(velocity);
	if(rightSpeed > 700)
		return;
	movingRight = true;
	isOnLeftWall = false;
	if(isOnGround)
	{
		float multiplier = MIN_MOVE_FORCE + (rightSpeed)/500 * MAX_MOVE_FORCE;
		if(multiplier > MIN_MOVE_FORCE + MAX_MOVE_FORCE)
			multiplier = MIN_MOVE_FORCE + MAX_MOVE_FORCE;
		else if(multiplier < abs(GRAVITATIONAL_FORCE) * mass)
			multiplier = abs(GRAVITATIONAL_FORCE) * mass;

		AddForce(right * multiplier * mass);
	}
	else if(isOnRightWall)
	{
		//AddForce(right * 500 * mass);
	}
	else
	{
		if(rightSpeed>100)
		{
			movingRight = false;
			return;
		}
		AddForce(right * AIR_MOVE_FORCE * mass);
	} 
}
void ControlledVertibrateObject::MoveLeft()
{
	isOnRightWall = false;
	Vector2D left = vertices[0] - vertices[1];
	left = !left;
	float leftSpeed = left.dot(velocity);
	if(leftSpeed > 700)
		return;
	movingLeft = true;
	if(isOnGround)
	{
		float multiplier = MIN_MOVE_FORCE + (leftSpeed)/500 * MAX_MOVE_FORCE;
		if(multiplier > MIN_MOVE_FORCE + MAX_MOVE_FORCE)
			multiplier = MIN_MOVE_FORCE + MAX_MOVE_FORCE;
		else if(multiplier < abs(GRAVITATIONAL_FORCE) * mass)
			multiplier = abs(GRAVITATIONAL_FORCE) * mass;
		AddForce(left * multiplier * mass);
	}
	else if(isOnLeftWall)
	{
		//AddForce(left * 500 * mass);
	}
	else
	{
		if(leftSpeed > 100)
		{
			movingLeft = false;
			return;
		}
		AddForce(left * AIR_MOVE_FORCE * mass);
	}
}
void ControlledVertibrateObject::Jump()
{
	if(isOnGround)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D up = Vector2D(0,1);//vertices[1] - vertices[2];
		up = !up * JUMP_FORCE * mass;
	
		AddForce(up);
	}
	else if(isOnRightWall)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D left = vertices[0] - vertices[1];
		left = (!left + Vector2D(0,1)) * 0.5;
		left = left * WALLJUMP_FORCE * mass;
		AddForce(left);
	}
	else if(isOnLeftWall)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D right = vertices[1] - vertices[0];
		right = (!right + Vector2D(0,1)) * 0.5;
		right = right * WALLJUMP_FORCE * mass;
		AddForce(right);
	}
	else
	{
	}
	isOnGround = false;
}
void ControlledVertibrateObject::AddGravity()
{
	Vector2D gravitationalForce = Vector2D(0.0, GRAVITATIONAL_FORCE) * mass;
	
	AddForce(gravitationalForce);
	ControlledObjectAddGravityEntryPoint();
}
bool ControlledVertibrateObject::VertibrateObjectPreThinkEntryPoint(double elapsedTime)
{
	ControlledObjectThinkEntryPoint(elapsedTime);
	return false;
}
void ControlledVertibrateObject::AddDamage(float damage)
{
	health-= damage;
}
void ControlledVertibrateObject::ThinkEntryPoint(double elapsedTime)
{
	if(!isOnGround && !isOnRightWall && !isOnLeftWall)
	{
		isOnGroundCount = 0;
		isOnLeftWallCount = 0;
		isOnRightWallCount = 0;
		isNotOnGroundCount++;
		if(!GetFreeRotate() && isNotOnGroundCount > 10)
		{
			Vector2D objectUp = vertices[1] - vertices[2];
			float bottomAngle = atan2(objectUp.y, objectUp.x);
			bottomAngle = 3.14159 * 0.5 - bottomAngle;

			if(bottomAngle > 0.1047)
				bottomAngle = 0.1047;
			else if(bottomAngle < -0.1047)
				bottomAngle = -0.1047;
				
			if(abs(bottomAngle) > 0.087267)
				Rotate(bottomAngle);
		}
		
	}
	else
	{
		bool canRotate = false;
		if(isOnLeftWall)
		{
			isOnLeftWallCount++;
			isNotOnGroundCount++;
			isOnRightWallCount = 0;
			isOnGroundCount = 0;
			if(isOnLeftWallCount > 5)
				canRotate = true;
			spriteList->Add(new Sprite(position + vertices[3], Vector2D(3,3), Vector2D(1,1), 0.05, textureManager, DUST_FILE));
		}
		else if(isOnRightWall)
		{
			isOnRightWallCount++;
			isNotOnGroundCount++;
			isOnGroundCount = 0;
			isOnLeftWallCount = 0;
			if(isOnRightWallCount > 5)
				canRotate = true;
			spriteList->Add(new Sprite(position + vertices[2], Vector2D(3,3), Vector2D(1,1), 0.05, textureManager, DUST_FILE));
		}
		else if(isOnGround)
		{
			isOnGroundCount ++;
			isNotOnGroundCount = 0;
			isOnLeftWallCount = 0;
			isOnRightWallCount = 0;
			if(isOnGroundCount > 5)
				canRotate = true;
		}
		
		if(rotateCount > 0 && !GetFreeRotate())
		{
			rotation = rotation/(float)rotateCount;

			lastRotation = rotation;
			if(rotation > 0.1047)
				rotation = 0.1047;
			else if(rotation < -0.1047)
				rotation = -0.1047;
			if(abs(rotation) > 0.087267)
			{
				Rotate(rotation);
			}
		}
	}
	
	if(GetFreeRotate())
	{
		rotation = angularVelocity * elapsedTime;
		while(rotation > 2 * 3.14159)
			rotation -= 2 * 3.14159;
		Rotate(rotation);
	}
	ControlledVertibrateObjectThinkEntryPoint(elapsedTime);
	spriteList->Cleanup();
	rotateCount = 0;
	rotation = 0.0;
	movingRight = false;
	movingLeft = false;
	isOnHead = false;
}
void ControlledVertibrateObject::ControlledVertibrateObjectThinkEntryPoint(double elapsedTime)
{
	
}
Vector2D ControlledVertibrateObject::VelocityAtPoint(Vector2D point)
{
	return velocity;
}
void ControlledVertibrateObject::ControlledObjectThinkEntryPoint(double elapsedTime)
{
}
void ControlledVertibrateObject::ControlledObjectDrawEntryPoint()
{
}
void ControlledVertibrateObject::ControlledObjectRotateEntryPoint(float cosTheta, float sinTheta)
{
}
void ControlledVertibrateObject::ControlledObjectAddGravityEntryPoint()
{
}
	